//
// Created by daybeha on 23-11-27.
//
#include <iostream>
#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Geometry>

#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "nav_msgs/msg/path.hpp"
#include "geometry_msgs/msg/pose_stamped.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include "tf2/LinearMath/Quaternion.h"

using namespace std;

class ImuToOdomTFConverter : public rclcpp::Node {
public:
    ImuToOdomTFConverter() : Node("imu_to_odom_tf_converter") {

        // imu_topic_ = this->declare_parameter("imu_topic", "/imu/sensor_msgs/Imu");
       imu_topic_ = this->declare_parameter("imu_topic", "/imu/data");
        cout << "subscribe imu_topic: " << imu_topic_ << endl;

        // have_orientation = this->declare_parameter("have_orientation", false);
       have_orientation = this->declare_parameter("have_orientation", true);
        cout << "have_orientation: " << have_orientation << endl;

        // 订阅IMU数据
        imu_subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
                imu_topic_, 10, std::bind(&ImuToOdomTFConverter::imuCallback, this, std::placeholders::_1));

        // 发布Odometry数据
        odom_publisher_ = this->create_publisher<nav_msgs::msg::Odometry>("/odom", 10);

        // 发布path数据
        path_publisher_ = this->create_publisher<nav_msgs::msg::Path>("/imu_path", 10);

        // 发布TF数据
        tf_broadcaster_ = std::make_shared<tf2_ros::TransformBroadcaster>(this);

        path.header.stamp = rclcpp::Time();
        path.header.frame_id="odom";
    }

private:
    void imuCallback(const sensor_msgs::msg::Imu::SharedPtr imu_msg) {
        // 处理IMU数据，进行必要的计算
        cout << "acc(gw): " << imu_msg->linear_acceleration.x << " " << imu_msg->linear_acceleration.y << " " << imu_msg->linear_acceleration.z << endl;
//    cout << "gyro: " << msg->angular_velocity.x << " " << msg->angular_velocity.y << " " << msg->angular_velocity.z << endl;
//    cout << "orientation: " << msg->orientation.w << " " << msg->orientation.x << " " << msg->orientation.y << " " << msg->orientation.z << endl;



        //ROS2中的时间类型builtin_interfaces::msg::Time是需要把秒和纳秒组合起来才能表示当前时间的
        double curr_time = imu_msg->header.stamp.sec + imu_msg->header.stamp.nanosec * 1e-9;
        Eigen::Vector3d curr_acc(imu_msg->linear_acceleration.x, imu_msg->linear_acceleration.y, imu_msg->linear_acceleration.z);
        Eigen::Vector3d curr_gyro(imu_msg->angular_velocity.x, imu_msg->angular_velocity.y, imu_msg->angular_velocity.z);
        if (have_orientation){
            Qwb = Eigen::Quaterniond(imu_msg->orientation.w, imu_msg->orientation.x, imu_msg->orientation.y, imu_msg->orientation.z);
        }


        Eigen::Vector3d dtheta_half, acc_w;
        if(pre_time!=0){
            double dt = curr_time - pre_time;
            Eigen::Vector3d gyro_w = Qwb * (curr_gyro + pre_gyro)/2.0;
            acc_w = Qwb * (curr_acc + pre_acc)/ 2.0 - gw;  // aw = Pwb * ( acc_body - acc_bias ) - gw
            cout << "acc: " << acc_w(0) << " " << acc_w(1) << " " << acc_w(2) << endl;

            // 路径积分
            Pwb = Pwb + Vw * dt + 0.5 * acc_w * dt * dt ;
            Vw = Vw + acc_w * dt;
            if(!have_orientation){
                dtheta_half =  (curr_gyro + pre_gyro) * dt /2.0 ;
                Qwb = Qwb * Eigen::Quaterniond(1, dtheta_half[0]/2,
                                               dtheta_half[1]/2, dtheta_half[2]/2).normalized();
            }


            /// 转为ros消息
            // 创建Odometry消息
            nav_msgs::msg::Odometry odom_msg;
            odom_msg.header.stamp = imu_msg->header.stamp;
            odom_msg.header.frame_id = "odom";
            odom_msg.child_frame_id = "base_link";
            // 创建TF消息
            geometry_msgs::msg::TransformStamped tf_msg;
            tf_msg.header = odom_msg.header;
            tf_msg.child_frame_id = odom_msg.child_frame_id;

            // 填充Twist信息
            odom_msg.twist.twist.linear.x = acc_w(0);
            odom_msg.twist.twist.linear.y = acc_w(1);
            odom_msg.twist.twist.linear.z = acc_w(2);
            odom_msg.twist.twist.angular.x = gyro_w(0);
            odom_msg.twist.twist.angular.y = gyro_w(1);
            odom_msg.twist.twist.angular.z = gyro_w(2);

            // 填充Pose信息
            add_pose(odom_msg, tf_msg);

            // 发布Path Odometry TFs消息
            path_publisher_->publish(path);
            odom_publisher_->publish(odom_msg);
            tf_broadcaster_->sendTransform(tf_msg);
        }

        pre_time = curr_time;
        pre_acc = curr_acc;
        pre_gyro = curr_gyro;
    }

    void add_pose(nav_msgs::msg::Odometry &odom_msg, geometry_msgs::msg::TransformStamped &tf_msg){
        geometry_msgs::msg::PoseStamped pose_stamped;
        pose_stamped.header.stamp = rclcpp::Time();
        pose_stamped.header.frame_id = "odom";

//        Pwb = Pwb/resize_factor;
        pose_stamped.pose.position.x = Pwb(0);
        pose_stamped.pose.position.y = Pwb(1);
        pose_stamped.pose.position.z = Pwb(2);
        tf_msg.transform.translation.x = Pwb(0);
        tf_msg.transform.translation.y = Pwb(1);
        tf_msg.transform.translation.z = Pwb(2);
        odom_msg.pose.pose.position = pose_stamped.pose.position;

        pose_stamped.pose.orientation.w = Qwb.w();
        pose_stamped.pose.orientation.x = Qwb.x();
        pose_stamped.pose.orientation.y = Qwb.y();
        pose_stamped.pose.orientation.z = Qwb.z();
        odom_msg.pose.pose.orientation = pose_stamped.pose.orientation;
        tf_msg.transform.rotation = pose_stamped.pose.orientation;

        path.poses.push_back(pose_stamped);
    }

    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr imu_subscription_;
    rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_publisher_;
    rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr path_publisher_;
    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

    string imu_topic_;

    nav_msgs::msg::Path path;
    // 初始化位姿
    Eigen::Vector3d Pwb = Eigen::Vector3d::Zero();
    Eigen::Quaterniond Qwb = Eigen::Quaterniond::Identity();
    Eigen::Vector3d Vw = Eigen::Vector3d::Zero();   // velocity in world space
//    Eigen::Vector3d gw = Eigen::Vector3d(-1.39375e-05, -1.3863e-06, 9.74734);          //  重力加速度（立放）
    Eigen::Vector3d gw = Eigen::Vector3d(0, 0, 9.82);          //  重力加速度(躺放)

    bool have_orientation;
    double pre_time;
    Eigen::Vector3d pre_acc;
    Eigen::Vector3d pre_gyro;
};

int main(int argc, char **argv) {
    rclcpp::init(argc, argv);
    auto node = std::make_shared<ImuToOdomTFConverter>();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
